#include "recognition.h"



float aceleration = 0.0, tmp_speed = 0.0;
float mean_speed = 0.0;
bool colecting = false, recognize = false;


trajectory frente;

void inicializa_template() {
	for (int i = 0; i < 20; i++)
	{
		frente.centroids[i].x = i;
		frente.centroids[i].y = 0;
	}
	frente.frame_count = 20;
}


void calculateAceleration(trajectory *trajectory_aux) {
	float dist_x = 0.0, dist_y = 0.0;
	if (trajectory_aux->frame_count > 1) {
		dist_x = DISTANCE2P(trajectory_aux->centroids[trajectory_aux->frame_count -1].x,
											trajectory_aux->centroids[trajectory_aux->frame_count - 2].x);
		dist_y = DISTANCE2P(trajectory_aux->centroids[trajectory_aux->frame_count -1].y,
											trajectory_aux->centroids[trajectory_aux->frame_count - 2].y);
		aceleration = sqrt(dist_x + dist_y) - tmp_speed;
		tmp_speed = sqrt(dist_x + dist_y);
	}
}

void calculateMeanSpeed (trajectory *trajectory_aux) {
	float dist_x = 0.0, dist_y = 0.0;
	if (trajectory_aux->frame_count > 1) {
		dist_x = DISTANCE2P(trajectory_aux->centroids[trajectory_aux->frame_count -1].x,
											trajectory_aux->centroids[trajectory_aux->frame_count - 2].x);
		dist_y = DISTANCE2P(trajectory_aux->centroids[trajectory_aux->frame_count -1].y,
											trajectory_aux->centroids[trajectory_aux->frame_count - 2].y);
		mean_speed += sqrt(dist_x+dist_y);
		mean_speed = mean_speed/(trajectory_aux->frame_count -1 );
	}
}

void gestureRecognition (trajectory *trajectory_aux){
	calculateAceleration(trajectory_aux);
	calculateMeanSpeed(trajectory_aux);
	if (!colecting){
		if (aceleration > ACELERATION_MIN) {
			colecting = true;
			trajectory_aux->frame_count = 0;
			printf ("Começou a coletar. Aceleracao = %f\n", aceleration);
		}
	}
	else {
		if (trajectory_aux->frame_count > TRAJECTORY_LENGTH) {
			printf("Movimento muito longo! Ignora-lo-ei.\n");
			colecting = false;
			aceleration = 0;
		}
		//printf("Velocidade Media = %f\n",mean_speed);
		if (mean_speed < MEAN_SPEED_MIN) {
			colecting = false;
			recognize = true;
			mean_speed = 0.0;
			printf("Parou de coletar, vai tentar reconhecer\n");
			printf("Frame_count = %d\n",trajectory_aux->frame_count);
		}
	}
	
	if ((recognize) &&  (trajectory_aux->frame_count > POINTS_NUM_MIN)){
		printf("Reconhecendo!\n");
		inicializa_template();
		trajectory temp = normalizeTrajectory(trajectory_aux);
		dtwRecognition(&temp, &frente);
		recognize = false;
	}
}

/* trajectory_aux ja tem que estar normalizado. TODO Tirar essa obrigacao? */
void dtwRecognition (trajectory *trajectory_aux, trajectory *gesture_template){
	
	int i,j;
	int matrix[TRAJECTORY_LENGTH][TRAJECTORY_LENGTH][2];
	
	if((trajectory_aux->frame_count < 1) || (gesture_template->frame_count < 1))
	{
		printf("AQUI\n");
		return;
	}
	matrix[trajectory_aux->frame_count][gesture_template->frame_count][0] = 0;
	matrix[trajectory_aux->frame_count][gesture_template->frame_count][1] = 0;

	/*Initialize first row and first column with zeros...*/
	matrix[0][0][0] = 0;
	matrix[0][0][1] = 0;
	for(i = 1; i < trajectory_aux->frame_count; i++){
			matrix[i][0][0] = trajectory_aux->centroids[i].x;
			matrix[i][0][1] = trajectory_aux->centroids[i].y;
	}

	for(j = 1; j < gesture_template->frame_count; j++){
			matrix[0][j][0] = gesture_template->centroids[j].x;
			matrix[0][j][1] = gesture_template->centroids[j].y;
	}
	//TODO Otimizar o algoritmo para armazenar apenas as duas ultimas colunas e nao toda a matriz...
	/* Apply recurrent rule.*/
	for(i = 1; i < trajectory_aux->frame_count; i++){
			for(j = 1; j < gesture_template->frame_count; j++){
					matrix[i][j][0] = (MIN3(matrix[i-1][j][0],matrix[i][j-1][0],matrix[i-1][j-1][0]) + abs(trajectory_aux->centroids[i].x - gesture_template->centroids[j].x));
					matrix[i][j][1] = (MIN3(matrix[i-1][j][1],matrix[i][j-1][1],matrix[i-1][j-1][1]) + abs(trajectory_aux->centroids[i].y - gesture_template->centroids[j].y));
			}
	}
	printf("DTW em X = %d\n",matrix[trajectory_aux->frame_count-1][gesture_template->frame_count-1][0]);
	printf("DTW em Y = %d\n",matrix[trajectory_aux->frame_count-1][gesture_template->frame_count-1][1]);
	/*Return result*/
	//*distance = (matrix[trajectory_aux->frame_count][gesture_template->frame_count][0] + matrix[trajectory_aux->frame_count][gesture_template->frame_count][1])/2;

	//return GR_ERROR_OK;
}